//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
//
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//  Copyright (C) Parallax, Inc. All Rights Reserved.
//
//  Updated Nov-2007 by Trevor Taylor, Software Technology, Australia
//
//  Change Log:
//  Implemented DriveDistance and RotateDegrees
//  Two different approaches are possible - just comment or uncomment
//  #define below for the one you want and recompile.
//
//
//  $File: BoeBotControl.cs $ $Revision: 10 $
//-----------------------------------------------------------------------

// The DriveDistance and RotateDegrees operations can work two ways.
// Uncomment the #define to use on-board motion behaviors.
// NOTE: You must download the monitor program that came with this
// service for this to work because it contains additional code!
// If you comment this out, then timers will be used.
//#define USE_MOTION_BEHAVIORS

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;

using boe = Microsoft.Robotics.Services.BasicStamp2.Proxy;
using dssphttp = Microsoft.Dss.Core.DsspHttp;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using model = Microsoft.Robotics.PhysicalModel.Proxy;
using W3C.Soap;
using System.ComponentModel;


namespace Robotics.BsDrive
{
    
    /// <summary>
    /// BsDrive Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract("http://schemas.microsoft.com/robotics/2006/05/drive.html")]
    [DisplayName("Boe-Bot Generic Drive")]
    [Description("Provides access to the Parallax BASIC Stamp 2 Boe-Bot drive service.\n(Uses the Generic Differential Drive contract.)\n(Partner with 'Boe-Bot BASIC Stamp 2').")]
    public class DriveService : DsspServiceBase
    {
        /// <summary>
        /// Microsoft.Robotics.Services.Drive.BsDrive Partner
        /// </summary>
        [Partner("SubMgr", Contract="http://schemas.microsoft.com/xw/2005/01/subscriptionmanager.html", CreationPolicy=PartnerCreationPolicy.CreateAlways, Optional=false)]
        private SubscriptionManagerPort _subMgrPort = new Microsoft.Dss.Services.SubscriptionManager.SubscriptionManagerPort();
        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/bsdrive", AllowMultipleInstances=false)]
        private drive.DriveOperations _mainPort = new Microsoft.Robotics.Services.Drive.Proxy.DriveOperations();
        /// <summary>
        /// Robotics.BsDrive.BsdriveService State
        /// </summary>
        [InitialStatePartner(Optional=true, ServiceUri="Robotics.BsDrive.BsdriveService.Config.xml")]
        private drive.DriveDifferentialTwoWheelState _state = new Microsoft.Robotics.Services.Drive.Proxy.DriveDifferentialTwoWheelState();

        /// <summary>
        /// Partner with the BasicStamp2 service
        /// </summary>
        [Partner("BASICStamp2", Contract = boe.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate, Optional = false)]
        private boe.BasicStamp2Operations _boebotPort = new boe.BasicStamp2Operations();

        
        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public DriveService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            InitializeState();

			base.Start();
        }

        #region Initialization 

        /// <summary>
        /// Initialize the drive state
        /// </summary>
        private void InitializeState()
        {
            if (_state == null)
            {
                _state = new drive.DriveDifferentialTwoWheelState();
            }

            if (_state.DistanceBetweenWheels == 0.0)
                _state.DistanceBetweenWheels = 0.105;

            _state.DriveState = drive.DriveState.Stopped;
            _state.IsEnabled = false;

            if (_state.LeftWheel == null)
                _state.LeftWheel = new motor.WheeledMotorState();
            if (_state.RightWheel == null)
                _state.RightWheel = new motor.WheeledMotorState();

            _state.LeftWheel.WheelSpeed = 0.0;
            _state.RightWheel.WheelSpeed = 0.0;

            if (_state.LeftWheel.GearRatio == 0.0)
                _state.LeftWheel.GearRatio = 1.0;
            if (_state.RightWheel.GearRatio == 0.0)
                _state.RightWheel.GearRatio = 1.0;

            if (_state.LeftWheel.Radius == 0.0)
                _state.LeftWheel.Radius = 0.033;
            if (_state.RightWheel.Radius == 0.0)
                _state.RightWheel.Radius = 0.033;

            if (_state.LeftWheel.MotorState == null)
                _state.LeftWheel.MotorState = new motor.MotorState("Left Wheel", 3, 1.0, false, new model.Pose());
            if (_state.LeftWheel.MotorState.HardwareIdentifier == 0)
                _state.LeftWheel.MotorState.HardwareIdentifier = 3;
            if (string.IsNullOrEmpty(_state.LeftWheel.MotorState.Name))
                _state.LeftWheel.MotorState.Name = "Left Wheel";
            if (_state.LeftWheel.MotorState.PowerScalingFactor == 0.0)
                _state.LeftWheel.MotorState.PowerScalingFactor = 1.0;

            if (_state.RightWheel.MotorState == null)
                _state.RightWheel.MotorState = new motor.MotorState("Right Wheel", 1, 1.0, true, new model.Pose());
            if (_state.RightWheel.MotorState.HardwareIdentifier == 0)
                _state.RightWheel.MotorState.HardwareIdentifier = 1;
            if (string.IsNullOrEmpty(_state.RightWheel.MotorState.Name))
                _state.RightWheel.MotorState.Name = "Right Wheel";
            if (_state.RightWheel.MotorState.PowerScalingFactor == 0.0)
                _state.RightWheel.MotorState.PowerScalingFactor = 1.0;

            SaveState(_state);
        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// HttpGet Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(Microsoft.Dss.Core.DsspHttp.HttpGet get)
        {
            get.ResponsePort.Post(new dssphttp.HttpResponseType(_state));
            yield break;
        }
        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(dssphttp.HttpPost submit)
        {
            // TODO: Implement Submit operations here.
            throw new NotImplementedException("TODO: Implement Submit operations here.");
        }
        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe)
        {
            base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield break;
        }
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(drive.Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }
        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateHandler(drive.Update update)
        {
            // TODO: Implement Update operations here.
            throw new NotImplementedException("TODO: Implement Update operations here.");
        }

        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual void EnableDriveHandler(drive.EnableDrive update)
        {
            _state.IsEnabled = update.Body.Enable;
            _state.TimeStamp = DateTime.Now;
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }

        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The Boe-Bot drive is not enabled.")));
                yield break;
            }

            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(update.Body.LeftWheelPower, update.Body.RightWheelPower),
                delegate(DefaultUpdateResponseType ok) 
                {
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault) 
                {
                    update.ResponsePort.Post(fault);
                });
                
        }

        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update)
        {
            if (!_state.IsEnabled)
            {
                update.ResponsePort.Post(Fault.FromException(new InvalidOperationException("The Boe-Bot drive is not enabled.")));
                yield break;
            }

            // TODO: Convert meters/second to BoeBot power based on radius of wheels.
            double leftPower = update.Body.LeftWheelSpeed;
            double rightPower = update.Body.RightWheelSpeed;

            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(leftPower, rightPower),
                delegate(DefaultUpdateResponseType ok)
                {
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                });
        }

        /// <summary>
        /// CalculateTimeToMove
        /// </summary>
        /// <param name="distance">Distance to travel</param>
        /// <param name="power">Drive power to use</param>
        /// <returns>Number of milliseconds</returns>
        public int CalculateTimeToMove(double distance, double power)
        {
            // TT Nov-2007
            // Drive distance is in meters so for now just convert to milliseconds
            // (which is equivalent to "millimeters" in the Dashboard user interface)
            //  Notice that the power is not used at all (yet)!
            //return (int)(Math.Abs(distance) * 1000.0);
            // Trial and error based on 8 x 45 degree turns
            // First guess -- Times 6
            // 5550 -- Not quite enough
            // 5800 -- Too much
            // 5675 -- Still too much
            // 5600, 5500? I dunno ...
            // Note that this will never be accurate because it is affected
            // by the individual motor characteristics and the battery power.
            // As the batteries run down, the distances will vary.

            // As the requested motor speed varies, so will the time required.
            // However, this is an inverse relationship. The assumption is that
            // it is linear, which of course it is not!
            // Most of the testing was at 300 in the Dashboard, which is 0.3 here.
            // Don't get caught by a (near) zero power -- this takes an infinite time!
            // In fact, the robot might not even budge for small power due to inertia.
            if (Math.Abs(power) < 0.01f)
                return 0;
            double powerFactor = 0.3f / Math.Abs(power);

#if USE_MOTION_BEHAVIORS
            return (int)(Math.Abs(distance) * 5500.0 * powerFactor);
#else
            return (int)(Math.Abs(distance) * 10000.0 * powerFactor);
#endif
        }

        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            // TT Nov-2007
            // This handler is basically the same as DriveDistance except for the
            // calculation of the distance. It could use DriveDistance, but that
            // is an unnecessary overhead.
            // See DriveDistance for more comments.

            // Time delay for the motion
            int delay;
            // Speeds for wheels to move at
            double leftSpeed;
            double rightSpeed;
            // Distance wheels will travel during rotation
            double arcDistance;

#if USE_MOTION_BEHAVIORS
            char code;
#endif

            // Check for zero degrees first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (update.Body.Degrees == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (update.Body.Degrees < 0.0)
            {
                // Note that the speeds are opposite to make the robot
                // rotate on the spot
                leftSpeed = update.Body.Power;
                rightSpeed = -update.Body.Power;
#if USE_MOTION_BEHAVIORS
                code = (char)boe.Behaviors.CLOCKWISE;       // 'C'
#endif
            }
            else
            {
                leftSpeed = -update.Body.Power;
                rightSpeed = update.Body.Power;
#if USE_MOTION_BEHAVIORS
                code = (char)boe.Behaviors.ANTI_CLOCKWISE;  // 'A'
#endif
            }

            // Calculate the distance that the wheels must travel for the given
            // turn angle. They will move around the circumference of a circle
            // with a diameter equal to the wheel base. Each 360 degrees of rotation
            // is one full circumference.
            // For the supposed wheelbase of 10.5cm this is 32.98672cm travelled
            // for a complete rotation
            arcDistance = Math.Abs(update.Body.Degrees) * _state.DistanceBetweenWheels * Math.PI / 360;

            // Calculate the delay time
            delay = CalculateTimeToMove(arcDistance, update.Body.Power);

            // There are two options for motions
#if USE_MOTION_BEHAVIORS
            // Either
            // 1. Let a behavior on the robot take care of it
            // The "magic" number 23 is because the servo pulses are supposed to be
            // every 20ms and there is some overhead in the looping
            yield return Arbiter.Choice(_boebotPort.ExecuteBehavior(code, delay / 23),
#else
            // Or
            // 2. Start the motors running (in opposite directions)
            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(leftSpeed, rightSpeed),
#endif
                delegate(DefaultUpdateResponseType ok)
                {
                    ;   // Nothing to do - just fall through
                },
                delegate(Fault fault)
                {
                    // Post the fault if the set speed failed
                    // This is not good error handling!
                    LogError("RotateDegrees - Could not start motors");
                    update.ResponsePort.Post(fault);
                });

            // Wait for the specified time period
            yield return Arbiter.Receive(
                false,
                TimeoutPort(delay),
                delegate(DateTime timeout) { });

            // Now stop the robot and post a response back to say the move is complete
            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(0.0, 0.0),
                delegate(DefaultUpdateResponseType ok)
                {
                    // Note that the response is after the move has finished
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    LogError("RotateDegrees - Could not stop motors");
                    update.ResponsePort.Post(fault);
                });
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            // TT Nov-2007
            // Drive Distance can be used to move a specified distance.
            // There are two approaches that can be used in the implementation:
            // 1. Tell the robot to move and let it do the timing by using an
            // onboard behavior. (It does not have any wheel encoders so it
            // can't measure distances.)
            // 2. Use a timer -- start the motors, wait, stop the motors.
            //
            // The problem with 1 is that the robot is effectively blind
            // while it is executing the command because it does not have
            // a multi-tasking operating system AND the BASICStamp is
            // responsible for pulsing the motors :-(  There is an add-on
            // device available from Parallax that takes over this task
            // so you do not have to tie up the processor. Most PICs on
            // the market have a PWM register and you can "set and forget",
            // but not the BASICStamp.
            //
            // The problem with 2 is that using timers on Windows is not
            // reliable. On top of that, there are transmission delays
            // when sending commands, to it is not possible to make very
            // accurate moves this way.
            //

            // Time delay for the motion
            int delay;
            // Speed to move at
            // NOTE: "speed" is really the power because Boe-Bot does not implement both.
            // Power values should be between 0 and 1.
            // The speed should be calculated based on the wheel radius and power,
            // but this is not implemented.
            double speed = Math.Abs(update.Body.Power);
            // Distance to travel
            // NOTE: Microsoft says that the Distance should always be POSITIVE and
            // that you use a negative Power to move backwards. I believe that a
            // convention of negative distances should be acceptable. Therefore
            // the code therefore checks both possibilities. This leads to the
            // strange case of a negative distance AND power, this is forwards!
            double distance = update.Body.Distance * Math.Sign(update.Body.Power);

#if USE_MOTION_BEHAVIORS
            char code;
#endif

            // Check for zero distance first because we don't want the wheels to jerk
            // if the robot is supposed to go nowhere!
            if (distance == 0.0)
            {
                update.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }
            else if (distance < 0.0)
            {
                // Speed needs to be negative if distance is negative
                speed = -speed;
#if USE_MOTION_BEHAVIORS
                code = (char)boe.Behaviors.BACKWARD;    // 'B'
#endif
            }
            else
            {
                // Speed remains the same (positive)
#if USE_MOTION_BEHAVIORS
                code = (char)boe.Behaviors.FORWARD;     // 'F'
#endif
            }

            // Calculate the delay time
            delay = CalculateTimeToMove(distance, speed);

            // Remember, there are two possible approaches.
            // The conditional compiler definition can be used to select
            // which one.
            //
#if USE_MOTION_BEHAVIORS
            // Either:
            // 1. Invoke a behavior on the robot itself ...
            yield return Arbiter.Choice(_boebotPort.ExecuteBehavior(code, delay / 23),
#else
            // Or
            // 2. Start the motors running!
            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(speed, speed),
#endif
                delegate(DefaultUpdateResponseType ok)
                {
                    ;   // Nothing to do - just fall through
                },
                delegate(Fault fault)
                {
                    // Post the fault if the set speed failed
                    // This is not good error handling!
                    LogError("DriveDistance - Could not start motors");
                    update.ResponsePort.Post(fault);
                });

            // Wait for the specified time period
            // If you are using approach 1, then it is not strictly necessary
            // to wait here. However, if you don't wait then this operation
            // will post a response and the caller might think that the move
            // has completed. It is safer just to wait in either case.
            yield return Arbiter.Receive(
                false,
                TimeoutPort(delay),
                delegate(DateTime timeout) { });

            // Now stop the robot and post a response back to say the move is complete
            // If you are using method 1 this should not be necessary, but it does
            // no harm to make sure it is stopped.
            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(0.0, 0.0),
                delegate(DefaultUpdateResponseType ok)
                {
                    // Only now, after the move has finished, post a response
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    LogError("DriveDistance - Could not stop motors");
                    update.ResponsePort.Post(fault);
                });
        }

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update)
        {
            yield return Arbiter.Choice(_boebotPort.UpdateMotorSpeed(0.0, 0.0),
                delegate(DefaultUpdateResponseType ok)
                {
                    update.ResponsePort.Post(ok);
                },
                delegate(Fault fault)
                {
                    update.ResponsePort.Post(fault);
                });
        }
        #endregion
    }

    /// <summary>
    /// BsDrive Contract
    /// </summary>
    public sealed class Contract
    {
        // TT - Modified the Contract Identifier for the new version
        /// The Unique Contract Identifier for the BsDrive service
        public const String Identifier = "http://schemas.microsoft.com/robotics/2007/10/bsdrive.html";
    }
}
